Control Theory

This notebook explore the dynamics and control of the cruise control system of a vehicle.

Cruise Control System

The Cruise Control is a system that automatically controls the speed of a motor vehicle. Applying some simplifications, we can describe the systems with the following block diagram.

Where the User inputs a desired Velocity [km/h]. This value is translated into bits [b] so it can be read by the Engine Control Unit (ECU). Where a compensator signals the Electric Throttle Controller (ETC) to determine a percentage [0-100%]. This signal is also in bits, so there should be a component that converts the signal into voltage [V] to control the Motor, this motor produces a driving force that impulses the vehicle forwards.

Now, there are external forces that act against the vehicle's motion. We call these forces Disturbances. The sum of these forces is the disturbance force, $F_{d}$. We can make a process out of Newton's second law, $F=ma$ to explain the Vehicle's dynamics. Where the final velocity can be obtained from the integral of the acceleration.

The feedback signal results from a Speedometer readings (which can also be affected), there should be also a conversor so the signal error can be generated from the difference between the desired and the actual velocity.

Dynamics

While this does not intend to be a comprehensive vehicle dynamics model, we can use a simplified model to simulate the behavior of a vehicle. The vehicle produces a force that propels it forward. This force, $F$, represents the vehicle's engine generated force.

We will define this force as:

$$F = \alpha \ T_{m} \ [1 - \beta \ (\frac{\omega}{\omega_{m}} - 1)^2] \ t$$

where:

• $\alpha$ is the gear ratio / wheel radius [1/m]
• $T_{m}$ is the maximum torque [Nm]
• $\beta$ is the peak engine rolloff [dimensionless]
• $\omega$ is the angular velocity [rad/s]
• $\omega_{m}$ is the vehicle's maximum angular velocity [rad/s]
• $t$ is the throttle input, a value between $0$ and $1$ [dimensionless]

Those values can be found in the vehicle's specifications.

This law also states that the acceleration of an object is directly proportional to the net force acting on it and inversely proportional to its mass. The law is given by the equation:

$$F = m \ a$$

For our vehicle model, the net force is the sum of the throttle force and the disturbance forces:

$$F_{net} = F - F_{d}$$

Then, the acceleration is given by:

$$a = \frac{F_{net}}{m}$$

Disturbance Forces

We will take into account three major forces that act against the vehicle's motion. The sum of these forces will the disturbance force, $F_{d}$.

$$F_{d} = F_{g} + F_{r} + F_{a}$$

Gravity

The force of gravity is given by:

$$F_{g} = m \ g \ sin(\theta)$$

where:

• $m$ is the mass of the vehicle
• $g$ is the acceleration due to gravity
• $\theta$ is the elevation angle of the road

Rolling Friction

Rolling friction is the force that opposes the motion of the vehicle. It is given by:

$$F_{r} = \mu \ N$$

where:

• $\mu$ is the coefficient of rolling friction
• $N$ is the normal force

Aerodynamic Drag Force

Drag depends on the properties of the fluid and on the size, shape, and speed of the object. One way to express this is by means of the drag equation:

$$F_{a} = \frac{1}{2} \ c_{d} \ A \ \rho \ v^2$$

where:

• $F_{d}$ is the drag force
• $\rho$ is the density of the fluid
• $v$ is the speed of the object relative to the fluid
• $A$ is the area of the object facing the fluid
• $c_{d}$ is the drag coefficient - a dimensionless number

As we will evaluate aerodynamics, the 'fluid' is the air.

At $101.325\ kPa$ (abs) and $15 °C$ ($59 °F$), air has a density of approximately $1.225 kg/m^3$ according to the International Standard Atmosphere (ISA).

Typical Drag Coefficients values

Object	Drag Coefficient ($c_{d}$)
Airfoil	0.05
Toyota Camry	0.28
Ford Focus	0.32
Honda Civic	0.36
Ferrari Testarossa	0.37
Dodge Ram pickup	0.43
Bicycle	0.9

Source: College Physics, OpenStax

from math import copysign, sin

import numpy as np
import pandas as pd
import seaborn as sns
from matplotlib import pyplot as plt
from pydantic import BaseModel

# Constants
air_density = 1.225  # kg/m^3
g = 9.80665  # m/s^2

Vehicle Model

We can model a simplified vehicle dynamics using a python class.

class Vehicle(BaseModel):
    """
    Vehicle Dynamics Model
    
    Attributes:
        position: current position of the vehicle (m)
        speed: current speed of the vehicle (m/s)
        mass: mass of the vehicle (Kg)
        max_throttle: maximum throttle force (N)
        drag_coefficient: drag coefficient (dimensionless)
        frontal_area: frontal area of the vehicle (m^2)
    """
    position: float = 0  # m
    speed: float = 0  # m/s
    mass: float  # Kg
    max_throttle: float  # N
    drag_coefficient: float  # dimensionless
    frontal_area: float  # m^2

Toyota Camry Example

Toyota Camry XSE 2025 (4T1DAACK2SU017407)

Specs:

2.5-Liter, 4-cylinder DOHC 16-valve, Variable Valve Timing-intelligence (VVT-i) with Variable Valve Timing-intelligence by Electric motor(VVT-iE) on the intake side, 183 hp @ 6000 rpm, 163 lb.-ft. @ 3600-5200 rpm: 2.5-Liter, 4-cylinder DOHC 16-valve, Variable Valve Timing-intelligence (VVT-i) with Variable Valve Timing-intelligence by Electric motor(VVT-iE) on the intake side, 183 hp @ 6000 rpm, 163 lb.-ft. @ 3600-5200 rpm

According to the Toyota specifications, this unit drag coefficient ($c_{d}$) is approximately $0.37$ and the frontal area ($A$) is $1.94 m^2$. It weighs $3538 \ lbs$ ($1604 \ kg$) and has a maximum horsepower of $184hp$ ($137209 \ W$), and maximum top speed of $130mph$ ($209.2km/h$ or $58.33 m/s$). It's maximum torque $T_{m}$ is $163 \ lb.-ft.$ ($221 \ Nm$), and peak engine angular velocity $\omega_{m}$ is $5200 \ rpm$ ($545.3 \ rad/s$).

camry = Vehicle(mass=1604, max_throttle=2352, drag_coefficient=0.37, frontal_area=1.94)
print(f"Toyota Camry XSE 2025({camry})")

Toyota Camry XSE 2025(position=0 speed=0 mass=1604.0 max_throttle=2352.0 drag_coefficient=0.37 frontal_area=1.94)

For our process or plant, we will use a simple model that simulates the vehicle dynamics. This model will have a method that updates the vehicle's position and speed based on the throttle input and the time step.

from math import radians

# Sign function
sign = lambda x: copysign(1, x)


def process(vehicle: Vehicle, throttle: float, dt: float, theta: float = 0.0, mu: float = 0.01) -> float:
    """
    Simulate vehicle dynamics updating its position and speed.
    
    Args:
        vehicle: a dynamic model
        throttle: percentage of throttle input
        dt: time step
        theta: inclination angle of the road
        mu: coefficient of rolling friction

    Returns:
        vehicle speed
    """
    m = vehicle.mass  # Kg
    v = vehicle.speed  # m/s
    area = vehicle.frontal_area  # m^2

    # Force generated by the throttle input
    f = throttle * vehicle.max_throttle

    # force Fg = m g sin \theta.
    fg = m * g * sin(radians(theta))

    # A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is
    # the coefficient of rolling friction and sgn(v) is the sign of v (±1) or
    # zero if v = 0.
    fr = m * g * mu * sign(v)

    # Fa = 1/2 * Cd * A * rho * v^2
    fa = 0.5 * vehicle.drag_coefficient * area * air_density * v ** 2

    # Total disturbance force
    fd = fg + fr + fa

    # a = F/m
    a = (f - fd) / m

    # Update vehicle position and speed
    vehicle.position += v * dt
    vehicle.speed += a * dt

    if vehicle.speed < 0:
        vehicle.speed = 0

    return vehicle.speed

We have built now a simple open-loop model of the vehicle dynamics.

class Simulation(BaseModel):
    times: list[float] = []
    speeds: list[float] = []
    positions: list[float] = []
    errors: list[float] = []
    inclinations: list[float] = []


def simulate(vehicle: Vehicle, total_time: float = 3_600.0, dt: float = 0.1) -> Simulation:
    sim = Simulation()

    # Reset vehicle state
    vehicle.position = 0
    vehicle.speed = 0

    for t in np.arange(0, total_time, dt):
        throttle = 0.75  # %
        speed = process(vehicle, throttle, dt)

        # Store simulation data
        sim.times.append(t)
        sim.speeds.append(speed)
        sim.positions.append(vehicle.position)

    return sim

def plot_results(simulation: Simulation, set_speed: float = 0.0):
    plt.clf()
    plt.close()
    plt.figure(figsize=(12, 6))
    plt.subplot(3, 1, 1)
    plt.plot(simulation.times, simulation.speeds, label='Speed')
    if set_speed > 0.0:
        plt.axhline(y=set_speed, color='r', linestyle='--', label='Set Speed')
    plt.xlabel('Time (s)')
    plt.ylabel('Speed (m/s)')
    plt.title('Vehicle Speed Over Time')
    plt.legend()

    plt.subplot(3, 1, 2)
    plt.plot(simulation.times, simulation.positions, label='Position')
    plt.xlabel('Time (s)')
    plt.ylabel('Position (m)')
    plt.title('Vehicle Position Over Time')
    plt.legend()

    if len(simulation.errors) > 0:
        plt.subplot(3, 1, 3)
        plt.plot(simulation.times, simulation.errors, label='Error')
        plt.xlabel('Time (s)')
        plt.ylabel('Error (m/s)')
        plt.title('Speed Error Over Time')
        plt.legend()

    plt.tight_layout()
    plt.show()


open_loop = simulate(camry)
plot_results(open_loop)

Nothing surprising here, as no control is applied, the vehicle accelerates until it reaches a maximum speed according to the throttle input and the disturbance forces acting on it.

Let's now implement a simple closed-loop system to control the speed of the vehicle.

Where the controller:

class PIDController(BaseModel):
    """
    Proportional-Integral-Derivative Controller
    
    Attributes:
        kp: proportional gain
        ki: integral gain
        kd: derivative gain
    """
    kp: float
    ki: float = 0.0
    kd: float = 0.0

    _integral: float = 0
    _previous_error: float = 0

    def update(self, error: float, dt: float) -> float:
        """
        Update PID controller output
        
        Args:
            error: signal error
            dt: time step
        
        Returns:
            A value between [-1, 1] control signal
        """
        self._integral += error * dt

        derivative = (error - self._previous_error) / dt

        # u(t) = Kp * e(t) + Ki * ∫e(t)dt + Kd * de(t)/dt
        output = self.kp * error + self.ki * self._integral + self.kd * derivative
        self._previous_error = error

        return np.clip(output, -1, 1)

Update the simulation function to include the controller.

from typing import Callable


def simulate(vehicle: Vehicle,
             set_speed: float,
             controller: Callable[[float, float], float],
             total_time: float = 3_600.0,
             dt: float = 0.1) -> Simulation:
    sim = Simulation()

    vehicle.speed = 0
    vehicle.position = 0

    for t in np.arange(0, total_time, dt):
        error = set_speed - vehicle.speed
        throttle = controller(error, dt)  # %
        speed = process(vehicle, throttle, dt)

        # Store simulation data
        sim.times.append(t)
        sim.speeds.append(speed)
        sim.positions.append(vehicle.position)
        sim.errors.append(error)

    return sim

Comparing Controllers

We will compare the performance of the controllers by setting a desired speed of $30 \ m/s$ (108 km/h) and evaluating the vehicle's response.

def calculate_settling_time(times: list, output: list, percentage: float = 0.05):
    """
    Calculate the settling time from the system output.

    Parameters:
    times (list): Recorded time of the system.
    output (list): System output.
    percentage (float): Tolerance percentage, by default 5%.

    Returns:
    float: settling time
    """
    final_value = output[-1]
    # bounds for 5% tolerance
    lower_bound = final_value * (1 - percentage)
    upper_bound = final_value * (1 + percentage)

    # find the first index where the output is within the bounds
    for i in reversed(range(len(output))):
        if not lower_bound <= output[i] <= upper_bound:
            # the last time the output was outside bounds + one step (we left the bounds)
            return times[i + 1]

    # output was always within the bounds
    return times[0]

p = PIDController(kp=1)
desired_speed = 30  # m/s (108 km/h)
p_simulation = simulate(camry, set_speed=desired_speed, controller=p.update)
plot_results(p_simulation, set_speed=desired_speed)

pi = PIDController(kp=1, ki=0.5)
pi_simulation = simulate(camry, set_speed=desired_speed, controller=pi.update)
plot_results(pi_simulation, set_speed=desired_speed)

pid = PIDController(kp=1, ki=0.5, kd=2)
pid_simulation = simulate(camry, set_speed=desired_speed, controller=pid.update)
plot_results(pid_simulation, set_speed=desired_speed)

t_p = calculate_settling_time(p_simulation.times, p_simulation.speeds)
t_pi = calculate_settling_time(pi_simulation.times, pi_simulation.speeds)
t_pid = calculate_settling_time(pid_simulation.times, pid_simulation.speeds)
print(f"Settling Time (P): {t_p:.2f} s")
print(f"Settling Time (PI): {t_pi:.2f} s")
print(f"Settling Time (PID): {t_pid:.2f} s")

Settling Time (P): 21.80 s
Settling Time (PI): 86.00 s
Settling Time (PID): 97.00 s

Disturbance

Let's add a disturbance to the system and evaluate the controllers' performance.

We will simulate a sudden change in the road inclination angle.

from scipy.stats import semicircular


def produce_inclination(prev_theta: float = 0.0,
                        min_theta: float = -5.0,
                        max_theta: float = 20.0,
                        ) -> float:
    # Random change in the inclination angle
    change = semicircular.rvs()

    # Update the inclination angle limiting its value
    theta = round(prev_theta + change, 2)

    return np.clip(theta, min_theta, max_theta)

current_theta = 0.0
inclinations = []
for _ in range(500):
    current_theta = produce_inclination(prev_theta=current_theta)
    inclinations.append(current_theta)

# Plotting
sns.histplot(inclinations, bins=400, kde=True)
plt.xlabel('Inclination')
plt.ylabel('Frequency')
plt.title('Histogram of Generated Road Inclinations')
plt.show()

Now, these inclinations change do not happen at regular intervals, so we will update the simulation function to include the disturbance.

We will use Maxwell distribution to generate random intervals between disturbances.

$$f(x) = \sqrt{2/\pi} x^2 e^{-x^2/2}$$

from scipy.stats import maxwell

fig, ax = plt.subplots(1, 1)
rv = maxwell()

x = np.linspace(maxwell.ppf(0.01),
                maxwell.ppf(0.99), 100)
ax.plot(x, maxwell.pdf(x), 'r-', lw=5, alpha=0.6, label='maxwell pdf')
ax.set_xlim([x[0], x[-1]])
plt.show()

def interval_generator() -> list[int]:
    """
    Generate random intervals between disturbances.
    """
    intervals = maxwell.rvs(loc=5, scale=750, size=3_600)
    return list(set(map(int, sorted(intervals))))

Update the simulation function to include the disturbance.

def adjust_inclination(theta: float = 0.0,
                       rate: float = 0.5,
                       ) -> float:
    if theta >= 0:
        return np.clip(theta - rate, 0, theta)
    elif theta < 0:
        return np.clip(theta + rate, theta, 0)


def generate_disturbances() -> dict:
    """
    Generate random disturbances.
    """
    times = interval_generator()
    return {t: produce_inclination() for t in times}


road_inclinations = generate_disturbances()


def simulate_with_disturbance(vehicle: Vehicle,
                              set_speed: float,
                              controller: Callable[[float, float], float],
                              total_time: float = 3_600.0,
                              dt: float = 1.0) -> Simulation:
    sim = Simulation()

    # Set Initial State
    vehicle.speed = 0.0
    vehicle.position = 0.0
    inclination_angle = 0.0
    t_last_disturbance = 0

    for t in np.arange(0, total_time, dt):

        # Check for disturbances
        if int(t) in road_inclinations.keys():
            inclination_angle = road_inclinations[int(t)]
            t_last_disturbance = t
        # Gradual change in the inclination angle after a disturbance to simulate the road leveling
        if t_last_disturbance + 2 > t:
            inclination_angle = adjust_inclination(inclination_angle)

        error = set_speed - vehicle.speed
        throttle = controller(error, dt)  # %
        speed = process(vehicle, throttle, dt, theta=inclination_angle)

        if speed > 70.0:
            print(
                f"SPEED Error at {t}s, speed: {speed} m/s, throttle: {throttle * 100} %, error: {error} m/s, inclination: {inclination_angle}")
            break

        # Store simulation data
        sim.times.append(t)
        sim.speeds.append(speed)
        sim.positions.append(vehicle.position)
        sim.errors.append(error)

    return sim

p_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=p.update, total_time=45 * 60)
plot_results(p_simulation, set_speed=desired_speed)

pi_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=pi.update, total_time=45 * 60)
plot_results(pi_simulation, set_speed=desired_speed)

pid_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=pid.update, total_time=45 * 60)
plot_results(pid_simulation, set_speed=desired_speed)

data = {
    "Simulation": ["P", "PI", "PID"],
    "Max Distance (km)": [max(p_simulation.positions) / 1_000, max(pi_simulation.positions) / 1_000,
                          max(pid_simulation.positions) / 1_000],
    "Average Speed": [sum(p_simulation.speeds) / len(p_simulation.speeds),
                      sum(pi_simulation.speeds) / len(pi_simulation.speeds),
                      sum(pid_simulation.speeds) / len(pid_simulation.speeds)],
    "Max Speed": [max(p_simulation.speeds), max(pi_simulation.speeds), max(pid_simulation.speeds)],
    "Min Speed": [min(p_simulation.speeds), min(pi_simulation.speeds), min(pid_simulation.speeds)],
    "Max Error": [max(p_simulation.errors), max(pi_simulation.errors), max(pid_simulation.errors)],
    "Min Error": [min(p_simulation.errors), min(pi_simulation.errors), min(pid_simulation.errors)]
}

pd.DataFrame(data)

	Simulation	Max Distance (km)	Average Speed	Max Speed	Min Speed	Max Error	Min Error
0	P	80.022734	29.649075	29.866562	1.368268	30.0	0.133438
1	PI	81.000000	30.011111	52.480769	1.368268	30.0	-22.480769
2	PID	80.999246	30.010498	52.480769	1.368268	30.0	-22.480769

Speedometer

The speedometer is a device that measures the instantaneous speed of a vehicle. While manufacturers have different technologies to measure speed, and calibrate their speedometers according to the vehicle's specifications, we can simulate a simple speedometer error as a percentage of the actual speed.

Let's simulate that the speedometer may have an error $+/- 2 km/h$, Which could translate to $0.5556 m/s$. For this simulation, we will consider a normal distribution for errors, and uniform random noise to simulate the speedometer's readings.

Also, Speedometers have a range of readings, so we will limit the readings between $0$ and $75 m/s$.

SPEEDOMETER_MIN = 0  # m/s
SPEEDOMETER_MAX = 75  # m/s


def measure_element(speed: float) -> float:
    """
    Measure the element with a 98% accuracy.
    """
    error = np.random.normal(0.2778, 0.5556)
    random = np.random.choice([1, -1])
    return np.clip(speed + random * error, SPEEDOMETER_MIN, SPEEDOMETER_MAX)

Let's see some examples of speedometer readings.

sample_measured_speeds = [measure_element(speed) for speed in p_simulation.speeds]

plt.figure(figsize=(12, 6))
plt.plot(p_simulation.times, p_simulation.speeds, label='Actual Speed')
plt.plot(p_simulation.times, sample_measured_speeds, label='Measured Speed')
plt.xlabel('Time (s)')
plt.ylabel('Speed (m/s)')
plt.title('Actual vs Measured Speed')
plt.legend()
plt.show()

speed_errors = [actual - measured for actual, measured in zip(p_simulation.speeds, sample_measured_speeds)]
plt.figure(figsize=(10, 6))
plt.plot(p_simulation.times, speed_errors)
plt.xlabel('Time (s)')
plt.ylabel('Speed Error (units as per speed)')
plt.title('Error in Speed Over Time')
plt.show()

plt.figure(figsize=(10, 6))
plt.hist(speed_errors, bins=30)
plt.xlabel('Speed Error (units as per speed)')
plt.ylabel('Frequency')
plt.title('Distribution of Speed Errors')
plt.show()

print(
    f"Percentage of less readings than actual speed: {len([e for e in speed_errors if e < 0]) / len(speed_errors) * 100:.2f}%")
print(
    f"Percentage of more readings than actual speed: {len([e for e in speed_errors if e > 0]) / len(speed_errors) * 100:.2f}%")

Percentage of less readings than actual speed: 49.89%
Percentage of more readings than actual speed: 50.11%

df_speedometer = pd.DataFrame({
    "Actual Speed": p_simulation.speeds,
    "Measured Speed": sample_measured_speeds,
    "Speed Error": speed_errors
})
df_speedometer.describe()

	Actual Speed	Measured Speed	Speed Error
count	2700.000000	2700.000000	2700.000000
mean	29.649075	29.649936	-0.000862
std	1.498468	1.620539	0.611654
min	1.368268	1.753615	-2.265078
25%	29.763037	29.354752	-0.392556
50%	29.767485	29.760115	0.002619
75%	29.771409	30.154742	0.402372
max	29.866562	31.970968	1.762274

df_speedometer.head()

	Actual Speed	Measured Speed	Speed Error
0	1.368268	2.524395	-1.156127
1	2.736022	1.753615	0.982407
2	4.102238	4.332648	-0.230410
3	5.465893	5.144739	0.321154
4	6.825972	6.327222	0.498750

Having the speedometer readings in mind, we can now say that our vehicle is in a stable state when the error is within the $4 km/h$ tolerance of the actual speed.

def ms_to_kmh(speed: float) -> float:
    """
    Speed conversion
    
    Args:
        speed: speed in m/s
    
    Returns:
        speed in km/h
    """
    return speed * 3.6

def simulate_with_readings(vehicle: Vehicle,
                           set_speed: float,
                           controller: Callable[[float, float], float],
                           total_time: float = 3_600.0,
                           dt: float = 1.0,
                           initial_speed: float = 0.0) -> Simulation:
    sim = Simulation()

    # Set Initial State
    vehicle.speed = initial_speed
    vehicle.position = 0.0
    inclination_angle = 0.0
    t_last_disturbance = 0

    for t in np.arange(0, total_time, dt):

        # Check for disturbances
        if int(t) in road_inclinations.keys():
            inclination_angle = road_inclinations[int(t)]
            t_last_disturbance = t
        # Gradual change in the inclination angle after a disturbance to simulate the road leveling
        if t_last_disturbance + 2 > t:
            inclination_angle = adjust_inclination(inclination_angle)

        # [H] - Measure the speed with the Speedometer
        read_speed = measure_element(vehicle.speed)

        # [e] - Calculate the error signal
        error = set_speed - read_speed

        # [Gc] - Apply the controller
        throttle = controller(error, dt)  # %

        # [Gp] - Process the vehicle dynamics
        speed = process(vehicle, throttle, dt, theta=inclination_angle)

        # Store simulation data
        sim.times.append(t)
        sim.speeds.append(speed)
        sim.positions.append(vehicle.position)
        sim.inclinations.append(inclination_angle)
        sim.errors.append(error)

    return sim

def to_km_simulation(sim: Simulation):
    """
    Convert the simulation speeds to km/h.
    
    Args:
        sim: Simulation data
    
    Returns:
        Simulation data with speeds in km/h
    """

    # Convert speeds to km/h
    sim.speeds = [ms_to_kmh(speed) for speed in sim.speeds]
    sim.errors = [ms_to_kmh(error) for error in sim.errors]

    # convert positions to km
    sim.positions = [pos / 1_000 for pos in sim.positions]

def plot_in_kmh(simulation: Simulation, set_speed: float = 0.0):
    """
    Plot the simulation results.
    
    Args:
        simulation: Simulation data
        set_speed: Speed in km/h
    """
    plt.clf()
    plt.close()
    plt.figure(figsize=(20, 15))
    plt.subplot(3, 1, 1)
    plt.plot(simulation.times, simulation.speeds, label='Actual Speed')
    if set_speed > 0.0:
        plt.axhline(y=set_speed + 4, color='greenyellow', linestyle='dotted', label='Speed Upper Bound')
        plt.axhline(y=set_speed, color='darkolivegreen', linestyle='--', label='Step Speed')
        plt.axhline(y=set_speed - 4, color='greenyellow', linestyle='dotted', label='Speed Lower Bound')
    plt.xlabel('Time (s)')
    plt.ylabel('Speed (km/h)')
    plt.title('Vehicle Speed Over Time')
    plt.legend()

    plt.subplot(3, 1, 2)
    plt.plot(simulation.times, simulation.inclinations, label='θ inclination')
    plt.xlabel('Time (s)')
    plt.ylabel('Inclination (Degrees)')
    plt.title('Road Inclination Over Time')
    plt.legend()

    if len(simulation.errors) > 0:
        plt.subplot(3, 1, 3)
        plt.plot(simulation.times, simulation.errors, label='Error')
        plt.xlabel('Time (s)')
        plt.ylabel('Error (km/h)')
        plt.title('Speed Error Over Time')
        plt.legend()

    plt.tight_layout()
    plt.show()

p_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=p.update, total_time=45 * 60)

to_km_simulation(p_simulation)

plot_in_kmh(p_simulation, set_speed=ms_to_kmh(desired_speed))

pi_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=pi.update, total_time=45 * 60)

to_km_simulation(pi_simulation)

plot_in_kmh(pi_simulation, set_speed=ms_to_kmh(desired_speed))

pid_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=pid.update, total_time=45 * 60)

to_km_simulation(pid_simulation)

plot_in_kmh(pid_simulation, set_speed=ms_to_kmh(desired_speed))

p_ctrl = PIDController(kp=1)

p_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=p_ctrl.update, total_time=45 * 60)
to_km_simulation(p_opt_sim)

plot_in_kmh(p_opt_sim, set_speed=ms_to_kmh(desired_speed))

pi_ctrl = PIDController(kp=3, ki=0.005)

pi_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=pi_ctrl.update, total_time=45 * 60)
to_km_simulation(pi_opt_sim)

plot_in_kmh(pi_opt_sim, set_speed=ms_to_kmh(desired_speed))

pid_ctrl = PIDController(kp=3, ki=0.005, kd=0.4)

pid_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=pid_ctrl.update, total_time=45 * 60)

to_km_simulation(pid_opt_sim)

plot_in_kmh(pid_opt_sim, set_speed=ms_to_kmh(desired_speed))

data = {
    "Simulation": ["P", "PI", "PID", "P (Optimized)", "PI (Optimized)", "PID (Optimized)"],
    "Max Distance (km)": [max(p_simulation.positions), max(pi_simulation.positions), max(pid_simulation.positions),
                          max(p_opt_sim.positions), max(pi_opt_sim.positions), max(pid_opt_sim.positions)],
    "Average Speed": [sum(p_simulation.speeds) / len(p_simulation.speeds),
                      sum(pi_simulation.speeds) / len(pi_simulation.speeds),
                      sum(pid_simulation.speeds) / len(pid_simulation.speeds),
                      sum(p_opt_sim.speeds) / len(p_opt_sim.speeds),
                      sum(pi_opt_sim.speeds) / len(pi_opt_sim.speeds),
                      sum(pid_opt_sim.speeds) / len(pid_opt_sim.speeds)],
    "Max Speed": [max(p_simulation.speeds), max(pi_simulation.speeds), max(pid_simulation.speeds),
                  max(p_opt_sim.speeds), max(pi_opt_sim.speeds), max(pid_opt_sim.speeds)],
    "Max Error": [max(p_simulation.errors), max(pi_simulation.errors), max(pid_simulation.errors),
                  max(p_opt_sim.errors), max(pi_opt_sim.errors), max(pid_opt_sim.errors)],
    "Min Error": [min(p_simulation.errors), min(pi_simulation.errors), min(pid_simulation.errors),
                  min(p_opt_sim.errors), min(pi_opt_sim.errors), min(pid_opt_sim.errors)],
    "Avg Error": [sum(p_simulation.errors) / len(p_simulation.errors),
                  sum(pi_simulation.errors) / len(pi_simulation.errors),
                  sum(pid_simulation.errors) / len(pid_simulation.errors),
                  sum(p_opt_sim.errors) / len(p_opt_sim.errors),
                  sum(pi_opt_sim.errors) / len(pi_opt_sim.errors), sum(pid_opt_sim.errors) / len(pid_opt_sim.errors)]
}

df_opt = pd.DataFrame(data)
df_opt

	Simulation	Max Distance (km)	Average Speed	Max Speed	Max Error	Min Error	Avg Error
0	P	79.732363	106.349316	114.848624	107.109215	-10.813715	1.662624
1	PI	81.037755	108.092624	186.657745	108.000000	-83.092243	0.002243
2	PID	81.018188	108.062117	188.930768	108.000000	-85.490061	-0.004419
3	P (Optimized)	79.755058	106.379970	114.122368	108.000000	-10.171372	1.654980
4	PI (Optimized)	80.847730	107.838082	116.807322	108.000000	-12.779878	0.291494
5	PID (Optimized)	80.727313	107.676281	116.468743	106.765584	-12.093140	0.379390

Control Analysis

Updating the simulation to use the refined package code.

from acc.utils.rv import RoadInclinationGenerator
from acc.model.control import EngineControlUnit
from acc.simulation import SimulationResult, run_simulation
from acc.model import process


def ctrl_simulation(
        kp: float = 0.5,
        ki: float = 0.2,
        kd: float = 1.0,
        windup_protection: bool = False,
        vi: float = 30.0,
        generate_inclinations: bool = False
) -> SimulationResult:
    """
    Run a simulation with a PID controller.
    """
    camry_xse_2025 = process.Vehicle(mass=1_604,
                                     drag_coefficient=0.28,
                                     frontal_area=1.94,
                                     torque_max=221,
                                     omega_max=545.3,
                                     gear_speed_ranges=[(0, 10), (10, 30), (30, 50), (50, 70), (70, 100), (100, 130),
                                                        (130, 160),
                                                        (160, 200)])
    ecu = EngineControlUnit(kp=kp, ki=ki, kd=kd, windup_protection=windup_protection)

    return run_simulation(vehicle=camry_xse_2025,
                          vi=vi,
                          control=ecu,
                          inclination_generator=RoadInclinationGenerator() if generate_inclinations else None,
                          initial_speed=0)

Running the simulation with a PID controller we notice that the speed reaches the upper limit for a long period before approaching the set speed.

from acc.utils import plot

results = ctrl_simulation(kp=2, ki=0.5, kd=4)

plot.plot_results(results, step_speed=30 * 3.6)

results.df().describe()

	Error	Speed	Throttle	Speedometer
count	3600.000000	3600.000000	3600.000000	3600.000000
mean	0.009106	107.980746	0.976328	107.990894
std	8.232561	7.942230	0.184040	8.232561
min	-7.705622	4.303984	-1.000000	0.838604
25%	-2.907126	107.556374	1.000000	107.394665
50%	-0.933405	108.701037	1.000000	108.933405
75%	0.605335	111.527989	1.000000	110.907126
max	107.161396	111.585574	1.000000	115.705622

Taking a loot at the controller output u(t). It is seen that for the exact same time that the speed reaches the upper limit, the controller output is at its maximum value. While the speed is inside the tolerance, is not quite the desired behavior.

from acc.simulation import SimulationResult


def time_to_steady_state(result: SimulationResult, step_speed=30 * 3.6):
    """
    Calculate the time to reach a steady state.
    
    Args:
        result: Simulation result
        step_speed: Step speed
    
    Returns:
        Time to reach a steady state
    """
    t = -1
    for i, speed in enumerate(result.speeds):
        if speed >= step_speed - 4:
            t = i
            break

    return t

print(f"Time to reach Steady State: {time_to_steady_state(results)} s")

Time to reach Steady State: 136 s

The integral problem

The integral control term adds up all the error amounts as time goes by. As long as a difference between $V_i$ and Vo exists, an error will continue accumulating. The faster the value accumulates, the more control output is added to the system since it indicates that we are far from the SP.

If it so happens that in a particular system, we cannot add enough control signal (throttle) to reach the $V_i$, the error will keep accumulating, there’s nothing that can be done.

If this happens, the error keeps building, and the internal calculation of the control system has produced an output of well over 100% of the full output signal. In our vehicle, the ‘throttle signal’ is telling you to push the pedal beyond the floor… But this is simply not possible.

The symptom of this effect is seen as a delay between the change in the throttle and the system response. For a while, the control signal is beyond its limit before returning to the range of real-world changes. It is a difficult symptom to diagnose because the reason for the delay is the past error accumulation driving the output too far in the present.

Solution

The simplest is to change the Integral term summation if the control output reaches 100%. If this output is reached, we know that the error will still accumulate, it’s unavoidable. We’re driving as fast as we can. In this case, it might be reasonable to halt the accumulation of the Integral term until the SP is changed again

Previous Controller

We accumulate the error in the integral term:

def control(error: float):        
    integral += error * dt
    
    derivative = (error - self._previous_error) / dt
    
    # u(t) = Kp * e(t) + Ki * ∫e(t)dt + Kd * de(t)/dt
    output = self.kp * error + self.ki * self._integral + self.kd * derivative
    self._previous_error = error
    
    return np.clip(output, -1, 1)

Windup Protection

Adding an optional windup protection to the controller. If the output is maxed out either brake (-100%) or accelerate (100%), stop accumulating error.

def control(error: float, windup: bool):        
    derivative = (error - self._previous_error) / dt
    
    # u(t) = Kp * e(t) + Ki * ∫e(t)dt + Kd * de(t)/dt
    output = self.kp * error + self.ki * self._integral + self.kd * derivative
    self._previous_error = error

    if self.windup_protection:
        # If output is maxed out either brake or accelerate, stop accumulating error
        if not (output == -1 and error < 0) and not (output == 1 and error > 0):
            self._integral += error * dt
    else:
        self._integral += error * dt
    
    return np.clip(output, -1, 1)

results = ctrl_simulation(kp=2, ki=0.5, kd=1.7, windup_protection=True)

plot.plot_results(results, step_speed=30 * 3.6)

The vehicle reaches the set speed in a shorter time, and the control output is within the limits, as we can see sooner throttle changes.

results.df().describe()

	Error	Speed	Throttle	Speedometer
count	3600.000000	3600.000000	3600.000000	3600.000000
mean	3.019558	104.989875	0.945361	104.980442
std	7.572387	7.306318	0.225944	7.572387
min	-2.241880	4.303984	-1.000000	0.701817
25%	1.092793	105.526554	1.000000	105.140010
50%	1.960259	106.117637	1.000000	106.039741
75%	2.859990	106.596135	1.000000	106.907207
max	107.298183	107.871560	1.000000	110.241880

Removing Integral control

With the issues caused by the integral term, one can think of removing the component...

results = ctrl_simulation(kp=1, ki=0, kd=0)

plot.plot_results(results, step_speed=30 * 3.6)

To no surprise, the vehicle never reaches the set speed, as without an accumulation of errors, it is never compensated in the steady state. The e(t) never reaches zero.

results.df().describe()

	Error	Speed	Throttle	Speedometer
count	3600.000000	3600.000000	3600.000000	3600.000000
mean	4.941723	103.082856	0.925511	103.058277
std	7.275457	7.009159	0.136840	7.275457
min	0.608632	4.303984	0.169064	0.000000
25%	3.236269	103.843895	0.898964	103.276018
50%	3.991878	104.075180	1.000000	104.008122
75%	4.723982	104.291125	1.000000	104.763731
max	108.000000	105.031663	1.000000	107.391368